﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2Distance.h
// Revision   : r112
// Change-Date: 2010-06-17
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2Distance.cpp
// Revision   : r132
// Change-Date: 2010-09-21
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// A distance proxy is used by the GJK algorithm.
    /// It encapsulates any shape.
    /// </summary>
    public struct DistanceProxy
    {
        /// <summary>
        /// Initialize the proxy using the given shape. The shape 
        /// must remain in scope while the proxy is in use. 
        /// </summary>
        public void Set(Shape shape, int index)
        {
            switch (shape.GetType())
            {
                case ShapeType.CIRCLE:
                    CircleShape circle = shape as CircleShape;
                    m_vertices = new Vector2[] { circle.m_p };
                    m_count = 1;
                    m_radius = circle.m_radius;
                    break;
                case ShapeType.POLYGON:
                    PolygonShape polygon = shape as PolygonShape;
                    m_vertices = polygon.m_vertices;
                    m_count = polygon.m_vertexCount;
                    m_radius = polygon.m_radius;
                    break;
                case ShapeType.LOOP:
                    LoopShape loop = shape as LoopShape;
                    System.Diagnostics.Debug.Assert(0 <= index && index < loop.Count);

                    if (m_buffer == null) m_buffer = new Vector2[2];
                    m_buffer[0] = loop.GetVertex(index);
                    if (index + 1 < loop.Count)
                    {
                        m_buffer[1] = loop.GetVertex(index + 1);
                    }
                    else
                    {
                        m_buffer[1] = loop.GetVertex(0);
                    }
                    m_vertices = m_buffer;
                    m_count = 2;
                    m_radius = loop.m_radius;
                    break;
                case ShapeType.EDGE:
                    EdgeShape edge = shape as EdgeShape;
                    m_vertices = new Vector2[] { edge.m_vertex1, edge.m_vertex2 };
                    m_count = 2;
                    m_radius = edge.m_radius;
                    break;
                default:
                    System.Diagnostics.Debug.Assert(false);
                    break;
            } 
        }

        /// <summary>
        /// Get the supporting vertex index in the given direction. 
        /// </summary>
        public Int32 GetSupport(Vector2 d)
        {
            Int32 bestIndex = 0;
            float bestValue = Vector2.Dot(m_vertices[0], d);
            for (Int32 i = 1; i < m_count; ++i)
            {
                float value = Vector2.Dot(m_vertices[i], d);
                if (value > bestValue)
                {
                    bestIndex = i;
                    bestValue = value;
                }
            }

            return bestIndex;
        }

        /// <summary>
        /// Get the supporting vertex index in the given direction. 
        /// </summary>
        public Int32 GetSupport(ref Vector2 d)
        {
            Int32 bestIndex = 0;
            float bestValue = Vector2.Dot(m_vertices[0], d);
            for (Int32 i = 1; i < m_count; ++i)
            {
                float value = Vector2.Dot(m_vertices[i], d);
                if (value > bestValue)
                {
                    bestIndex = i;
                    bestValue = value;
                }
            }

            return bestIndex;
        }

        /// <summary>
        /// Get the supporting vertex in the given direction. 
        /// </summary>
        public Vector2 GetSupportVertex(Vector2 d)
        {
            Int32 bestIndex = 0; 
            float bestValue = Vector2.Dot(m_vertices[0], d); 
            for (Int32 i = 1; i < m_count; ++i) 
            { 
                float value = Vector2.Dot(m_vertices[i], d); 
                if (value > bestValue) 
                { 
                    bestIndex = i;
                    bestValue = value; 
                } 
            } 
            
            return m_vertices[bestIndex];
        }

        /// <summary>
        /// Get the vertex count. 
        /// </summary>
        public Int32 GetVertexCount()
        {
            return m_count;
        }

        /// <summary>
        /// Get a vertex by index. Used by Distance. 
        /// </summary>
        public Vector2 GetVertex(ref Int32 index)
        {
            System.Diagnostics.Debug.Assert(0 <= index && index <= m_count);
            return m_vertices[index];
        }

        /// <summary>
        /// Get a vertex by index. Used by Distance. 
        /// </summary>
        public Vector2 GetVertex(Int32 index)
        {
            System.Diagnostics.Debug.Assert(0 <= index && index <= m_count);
            return m_vertices[index];
        }

        Vector2[] m_buffer;
        public Vector2[] m_vertices; 
        public Int32 m_count; 
        public float m_radius; 
    }

    /// <summary>
    /// Used to warm start Distance.
    /// Set count to zero on first call.
    /// </summary>
    public struct SimplexCache
    {
        public float metric;	    /// length or area
        public int count;
        public IndexArray indexA;	/// vertices on shape A
        public IndexArray indexB;	/// vertices on shape B
    };

    public struct IndexArray
    {
        private Byte I0, I1, I2;

        public Byte this[int index]
        {
            get
            {
                System.Diagnostics.Debug.Assert(index >= 0 && index < 3);
                if (index == 0) return I0;
                else if (index == 1) return I1;
                else return I2;
            }
            set
            {
                System.Diagnostics.Debug.Assert(index >= 0 && index < 3);
                if (index == 0) I0 = value;
                else if (index == 1) I1 = value;
                else I2 = value;
            }
        }
    }

    /// <summary>
    /// Input for Distance.
    /// You have to option to use the shape radii
    /// in the computation. Even 
    /// </summary>
    public struct DistanceInput
    {
        public DistanceProxy proxyA;
        public DistanceProxy proxyB;
        public Transform transformA;
        public Transform transformB;
        public bool useRadii;
    };

    /// <summary>
    /// Output for Distance.
    /// </summary>
    public struct DistanceOutput
    {
        public Vector2 pointA;		/// closest point on shapeA
        public Vector2 pointB;		/// closest point on shapeB
        public float distance;
        public int iterations;	    /// number of GJK iterations used
    };

    // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. 

    internal struct SimplexVertex
    {
        internal Vector2 wA;    // support point in shapeA 
        internal Vector2 wB;    // support point in shapeB 
        internal Vector2 w;     // wB - wA 
        internal float a;       // barycentric coordinate for closest point 
        internal int indexA;    // wA index 
        internal int indexB;    // wB index 
    }

    internal struct Simplex
    {
#if !(ZUNE || WINDOWS_PHONE || SILVERLIGHT)
        internal unsafe void ReadCache(ref SimplexCache cache,
                                ref DistanceProxy proxyA, ref Transform transformA,
                                ref DistanceProxy proxyB, ref Transform transformB)
        {
            System.Diagnostics.Debug.Assert(cache.count <= 3);

            fixed (SimplexVertex* vertices = &m_v1)
            {
                // Copy data from cache.
                m_count = cache.count;

                for (int i = 0; i < m_count; ++i)
                {
                    SimplexVertex* v = vertices + i;
                    v->indexA = cache.indexA[i];
                    v->indexB = cache.indexB[i];
                    Vector2 wALocal = proxyA.GetVertex(v->indexA);
                    Vector2 wBLocal = proxyB.GetVertex(v->indexB);
#if !OPTIMIZE
                    v->wA = Math.Mul(transformA, wALocal);
                    v->wB = Math.Mul(transformB, wBLocal);
#else
                    Vector2 t; t.X = transformA.R.col1.X * wALocal.X + transformA.R.col2.X * wALocal.Y; t.Y = transformA.R.col1.Y * wALocal.X + transformA.R.col2.Y * wALocal.Y;
                    v->wA.X = transformA.position.X + t.X;
                    v->wA.Y = transformA.position.Y + t.Y;

                    t.X = transformB.R.col1.X * wBLocal.X + transformB.R.col2.X * wBLocal.Y; t.Y = transformB.R.col1.Y * wBLocal.X + transformB.R.col2.Y * wBLocal.Y;
                    v->wB.X = transformB.position.X + t.X;
                    v->wB.Y = transformB.position.Y + t.Y;
#endif
                    v->w = v->wB - v->wA;
                    v->a = 0.0f;
                }

                // Compute the new simplex metric, if it is substantially different than
                // old metric then flush the simplex.
                if (m_count > 1)
                {
                    float metric1 = cache.metric;
                    float metric2 = GetMetric();
                    if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < PhysicSettings.FLT_EPSILON)
                    {
                        // Reset the simplex.
                        m_count = 0;
                    }
                }

                // If the cache is empty or invalid ...
                if (m_count == 0)
                {
                    SimplexVertex* v = vertices + 0;
                    v->indexA = 0;
                    v->indexB = 0;
                    Vector2 wALocal = proxyA.GetVertex(0);
                    Vector2 wBLocal = proxyB.GetVertex(0);
#if !OPTIMIZE
                    v->wA = Math.Mul(transformA, wALocal);
                    v->wB = Math.Mul(transformB, wBLocal);
#else
                    Vector2 t; t.X = transformA.R.col1.X * wALocal.X + transformA.R.col2.X * wALocal.Y; t.Y = transformA.R.col1.Y * wALocal.X + transformA.R.col2.Y * wALocal.Y;
                    v->wA.X = transformA.position.X + t.X;
                    v->wA.Y = transformA.position.Y + t.Y;

                    t.X = transformB.R.col1.X * wBLocal.X + transformB.R.col2.X * wBLocal.Y; t.Y = transformB.R.col1.Y * wBLocal.X + transformB.R.col2.Y * wBLocal.Y;
                    v->wB.X = transformB.position.X + t.X;
                    v->wB.Y = transformB.position.Y + t.Y;
#endif
                    v->w = v->wB - v->wA;
                    m_count = 1;
                }
            }
        }

        internal unsafe void WriteCache(ref SimplexCache cache)
        {
            fixed (SimplexVertex* vertices = &m_v1)
            {
                cache.metric = GetMetric();
                cache.count = (ushort)m_count;
                
                for (int i = 0; i < m_count; ++i)
                {
                    cache.indexA[i] = (byte)(vertices[i].indexA);
                    cache.indexB[i] = (byte)(vertices[i].indexB);
                }
            }
            // no write back necessary
        }
#else
        internal void ReadCache(ref SimplexCache cache,
                                ref DistanceProxy proxyA, ref Transform transformA,
                                ref DistanceProxy proxyB, ref Transform transformB)
        {
            System.Diagnostics.Debug.Assert(cache.count <= 3);

            SimplexVertex[] vertices = { m_v1, m_v2, m_v3 };
            // Copy data from cache.
            m_count = cache.count;

            for (int i = 0; i < m_count; ++i)
            {
                SimplexVertex v = vertices[i];
                v.indexA = cache.indexA[i];
                v.indexB = cache.indexB[i];
                Vector2 wALocal = proxyA.GetVertex(v.indexA);
                Vector2 wBLocal = proxyB.GetVertex(v.indexB);
#if !OPTIMIZE
                v->wA = Math.Mul(transformA, wALocal);
                v->wB = Math.Mul(transformB, wBLocal);
#else
                Vector2 t; t.X = transformA.R.col1.X * wALocal.X + transformA.R.col2.X * wALocal.Y; t.Y = transformA.R.col1.Y * wALocal.X + transformA.R.col2.Y * wALocal.Y;
                v.wA.X = transformA.position.X + t.X;
                v.wA.Y = transformA.position.Y + t.Y;

                t.X = transformB.R.col1.X * wBLocal.X + transformB.R.col2.X * wBLocal.Y; t.Y = transformB.R.col1.Y * wBLocal.X + transformB.R.col2.Y * wBLocal.Y;
                v.wB.X = transformB.position.X + t.X;
                v.wB.Y = transformB.position.Y + t.Y;
#endif
                v.w = v.wB - v.wA;
                v.a = 0.0f;
                vertices[i] = v;
            }

            // Compute the new simplex metric, if it is substantially different than
            // old metric then flush the simplex.
            if (m_count > 1)
            {
                float metric1 = cache.metric;
                float metric2 = GetMetric();
                if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < PhysicSettings.FLT_EPSILON)
                {
                    // Reset the simplex.
                    m_count = 0;
                }
            }

            // If the cache is empty or invalid ...
            if (m_count == 0)
            {
                SimplexVertex v = vertices[0];
                v.indexA = 0;
                v.indexB = 0;
                Vector2 wALocal = proxyA.GetVertex(0);
                Vector2 wBLocal = proxyB.GetVertex(0);
#if !OPTIMIZE
                v.wA = Math.Mul(transformA, wALocal);
                v.wB = Math.Mul(transformB, wBLocal);
#else
                Vector2 t; t.X = transformA.R.col1.X * wALocal.X + transformA.R.col2.X * wALocal.Y; t.Y = transformA.R.col1.Y * wALocal.X + transformA.R.col2.Y * wALocal.Y;
                v.wA.X = transformA.position.X + t.X;
                v.wA.Y = transformA.position.Y + t.Y;

                t.X = transformB.R.col1.X * wBLocal.X + transformB.R.col2.X * wBLocal.Y; t.Y = transformB.R.col1.Y * wBLocal.X + transformB.R.col2.Y * wBLocal.Y;
                v.wB.X = transformB.position.X + t.X;
                v.wB.Y = transformB.position.Y + t.Y;
#endif
                v.w = v.wB - v.wA;
                m_count = 1;
                vertices[0] = v;
            }
        }

        internal void WriteCache(ref SimplexCache cache)
        {
            SimplexVertex[] vertices = { m_v1, m_v2, m_v3 };
            {
                cache.metric = GetMetric();
                cache.count = (ushort)m_count;
                
                for (int i = 0; i < m_count; ++i)
                {
                    cache.indexA[i] = (byte)(vertices[i].indexA);
                    cache.indexB[i] = (byte)(vertices[i].indexB);
                }
            }
            // no write back necessary
        }
#endif

        internal Vector2 GetSearchDirection()
        {
            switch (m_count)
            {
                case 1:
                    return -m_v1.w;

                case 2:
                    {
                        Vector2 e12 = m_v2.w - m_v1.w;
                        float sgn = Vector2Extensions.Cross(e12, -m_v1.w);
                        if (sgn > 0.0f)
                        {
                            // Origin is left of e12.
                            return Vector2Extensions.Cross(1.0f, e12);
                        }
                        else
                        {
                            // Origin is right of e12.
                            return Vector2Extensions.Cross(e12, 1.0f);
                        }
                    }

                default:
                    System.Diagnostics.Debug.Assert(false);
                    return Vector2.Zero;
            }
        }

        internal Vector2 GetClosestPoint()
        {
            switch (m_count)
            {
                case 0:
                    System.Diagnostics.Debug.Assert(false);
                    return Vector2.Zero;

                case 1:
                    return m_v1.w;

                case 2:
                    return m_v1.a * m_v1.w + m_v2.a * m_v2.w;

                case 3:
                    return Vector2.Zero;

                default:
                    System.Diagnostics.Debug.Assert(false);
                    return Vector2.Zero;
            }
        }

        internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB)
        {
            switch (m_count)
            {
                default:
                case 0:
                    pA = Vector2.Zero;
                    pB = Vector2.Zero;
                    System.Diagnostics.Debug.Assert(false);
                    break;

                case 1:
                    pA = m_v1.wA;
                    pB = m_v1.wB;
                    break;

                case 2:
                    pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
                    pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
                    break;

                case 3:
                    pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
                    pB = pA;
                    break;
            }
        }

        private float GetMetric()
        {
            switch (m_count)
            {
                case 0:
                    System.Diagnostics.Debug.Assert(false);
                    return 0.0f;

                case 1:
                    return 0.0f;

                case 2:
                    return (m_v1.w - m_v2.w).Length();  ////////TODO: is this really correct? original is b2Distance(m_v1.w, m_v2.w);

                case 3:
                    return Vector2Extensions.Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);

                default:
                    System.Diagnostics.Debug.Assert(false);
                    return 0.0f;
            }
        }

        /// <summary>
        // Solve a line segment using barycentric coordinates.
        //
        // p = a1 * w1 + a2 * w2
        // a1 + a2 = 1
        //
        // The vector from the origin to the closest point on the line is
        // perpendicular to the line.
        // e12 = w2 - w1
        // dot(p, e) = 0
        // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
        //
        // 2-by-2 linear system
        // [1      1     ][a1] = [1]
        // [w1.e12 w2.e12][a2] = [0]
        //
        // Define
        // d12_1 =  dot(w2, e12)
        // d12_2 = -dot(w1, e12)
        // d12 = d12_1 + d12_2
        //
        // Solution
        // a1 = d12_1 / d12
        // a2 = d12_2 / d12
        /// </summary>
        internal void Solve2()
        {
            Vector2 w1 = m_v1.w;
            Vector2 w2 = m_v2.w;
            Vector2 e12 = w2 - w1;

            // w1 region
            float d12_2 = -Vector2.Dot(w1, e12);
            if (d12_2 <= 0.0f)
            {
                // a2 <= 0, so we clamp it to 0
                m_v1.a = 1.0f;
                m_count = 1;
                return;
            }

            // w2 region
            float d12_1 = Vector2.Dot(w2, e12);
            if (d12_1 <= 0.0f)
            {
                // a1 <= 0, so we clamp it to 0
                m_v2.a = 1.0f;
                m_count = 1;
                m_v1 = m_v2;
                return;
            }

            // Must be in e12 region.
            float inv_d12 = 1.0f / (d12_1 + d12_2);
            m_v1.a = d12_1 * inv_d12;
            m_v2.a = d12_2 * inv_d12;
            m_count = 2;
        }

        /// <summary>
        // Possible regions:
        // - points[2]
        // - edge points[0]-points[2]
        // - edge points[1]-points[2]
        // - inside the triangle
        /// </summary>
        internal void Solve3()
        {
            Vector2 w1 = m_v1.w;
            Vector2 w2 = m_v2.w;
            Vector2 w3 = m_v3.w;

            // Edge12
            // [1      1     ][a1] = [1]
            // [w1.e12 w2.e12][a2] = [0]
            // a3 = 0
            Vector2 e12 = w2 - w1;
            float w1e12 = Vector2.Dot(w1, e12);
            float w2e12 = Vector2.Dot(w2, e12);
            float d12_1 = w2e12;
            float d12_2 = -w1e12;

            // Edge13
            // [1      1     ][a1] = [1]
            // [w1.e13 w3.e13][a3] = [0]
            // a2 = 0
            Vector2 e13 = w3 - w1;
            float w1e13 = Vector2.Dot(w1, e13);
            float w3e13 = Vector2.Dot(w3, e13);
            float d13_1 = w3e13;
            float d13_2 = -w1e13;

            // Edge23
            // [1      1     ][a2] = [1]
            // [w2.e23 w3.e23][a3] = [0]
            // a1 = 0
            Vector2 e23 = w3 - w2;
            float w2e23 = Vector2.Dot(w2, e23);
            float w3e23 = Vector2.Dot(w3, e23);
            float d23_1 = w3e23;
            float d23_2 = -w2e23;

            // Triangle123
            float n123 = Vector2Extensions.Cross(e12, e13);

            float d123_1 = n123 * Vector2Extensions.Cross(w2, w3);
            float d123_2 = n123 * Vector2Extensions.Cross(w3, w1);
            float d123_3 = n123 * Vector2Extensions.Cross(w1, w2);

            // w1 region
            if (d12_2 <= 0.0f && d13_2 <= 0.0f)
            {
                m_v1.a = 1.0f;
                m_count = 1;
                return;
            }

            // e12
            if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
            {
                float inv_d12 = 1.0f / (d12_1 + d12_2);
                m_v1.a = d12_1 * inv_d12;
                m_v2.a = d12_2 * inv_d12;
                m_count = 2;
                return;
            }

            // e13 
            if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
            {
                float inv_d13 = 1.0f / (d13_1 + d13_2);
                m_v1.a = d13_1 * inv_d13;
                m_v3.a = d13_2 * inv_d13;
                m_count = 2;
                m_v2 = m_v3;
                return;
            }

            // w2 region 
            if (d12_1 <= 0.0f && d23_2 <= 0.0f)
            {
                m_v2.a = 1.0f;
                m_count = 1;
                m_v1 = m_v2;
                return;
            }

            // w3 region 
            if (d13_1 <= 0.0f && d23_1 <= 0.0f)
            {
                m_v3.a = 1.0f;
                m_count = 1;
                m_v1 = m_v3;
                return;
            }


            // e23 
            if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
            {
                float inv_d23 = 1.0f / (d23_1 + d23_2);
                m_v2.a = d23_1 * inv_d23;
                m_v3.a = d23_2 * inv_d23;
                m_count = 2;
                m_v1 = m_v3;
                return;
            }

            // Must be in triangle123 
            float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
            m_v1.a = d123_1 * inv_d123;
            m_v2.a = d123_2 * inv_d123;
            m_v3.a = d123_3 * inv_d123;
            m_count = 3; 
        }

        internal SimplexVertex m_v1, m_v2, m_v3;
        internal int m_count;
    }

    public partial class Collision
    {
        public static Int32 gjkCalls;
        public static Int32 gjkIters;
        public static Int32 gjkMaxIters;

#if !ZUNE && !WINDOWS_PHONE && !SILVERLIGHT
        /// <summary> 
        /// Compute the closest points between two shapes. Supports any combination of: 
        /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output. 
        /// On the first call set SimplexCache.Count to zero. 
        /// </summary>           
        public unsafe static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input)
        {
            output = new DistanceOutput();

            ++gjkCalls;

            DistanceProxy proxyA = input.proxyA;
            DistanceProxy proxyB = input.proxyB;

            Transform transformA = input.transformA;
            Transform transformB = input.transformB;

            // Initialize the simplex. 
            Simplex simplex = new Simplex();
            simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);

            // Get simplex vertices as an array. 
            //SimplexVertex[] vertices = { simplex.m_v1, simplex.m_v2, simplex.m_v3 };
            SimplexVertex* vertices = &simplex.m_v1;
            const int k_maxIters = 20;

            // These store the vertices of the last simplex so that we 
            // can check for duplicates and prevent cycling. 
            int[] saveA = new int[3], saveB = new int[3];
            int saveCount = 0;

            Vector2 closestPoint = simplex.GetClosestPoint();
            float distanceSqr1 = closestPoint.LengthSquared();
            float distanceSqr2 = distanceSqr1;

            // Main iteration loop.
            int iter = 0;
            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates. 
                saveCount = simplex.m_count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = vertices[i].indexA;
                    saveB[i] = vertices[i].indexB;
                }

                switch (simplex.m_count)
                {
                    case 1:
                        break;

                    case 2:
                        simplex.Solve2();
                        break;

                    case 3:
                        simplex.Solve3();
                        break;

                    default:
                        System.Diagnostics.Debug.Assert(false);
                        break;
                }

                // If we have 3 points, then the origin is in the corresponding triangle. 
                if (simplex.m_count == 3)
                {
                    break;
                }

                // Compute closest point. 
                Vector2 p = simplex.GetClosestPoint();
                distanceSqr2 = p.LengthSquared();

                // Ensure the search direction is numerically fit. 
                //if (distanceSqr2 >= distanceSqr1)
                //{
                    //break;
                //}
                distanceSqr1 = distanceSqr2;

                // Get search direction
                Vector2 d = simplex.GetSearchDirection();

                if (d.LengthSquared() < PhysicSettings.FLT_EPSILON_SQUARED)
                {
                    // The origin is probably contained by a line segment 
                    // or triangle. Thus the shapes are overlapped. 

                    // We can't return zero here even though there may be overlap. 
                    // In case the simplex is a point, segment, or triangle it is difficult 
                    // to determine if the origin is contained in the CSO or very close to it. 
                    break;
                }

                // Compute a tentative new simplex vertex using support points. 
                SimplexVertex* vertex = vertices + simplex.m_count;
#if !OPTIMIZE
                vertex->indexA = proxyA.GetSupport(Math.MulT(transformA.R, -d));
                vertex->wA = Math.Mul(transformA, proxyA.GetVertex(vertex->indexA));
                //Vector2 wBLocal = new Vector2(); 
                vertex->indexB = proxyB.GetSupport(Math.MulT(transformB.R, d));
                vertex->wB = Math.Mul(transformB, proxyB.GetVertex(vertex->indexB));
                vertex->w = vertex->wB - vertex->wA;
                //vertices[simplex.m_count] = vertex;
#else
                Vector2 t, sv, gs = Vector2.Zero, nd;

                nd = -d;
                Vector2.Dot(ref nd, ref transformA.R.col1, out gs.X);
                Vector2.Dot(ref nd, ref transformA.R.col2, out gs.Y);
                vertex->indexA = proxyA.GetSupport(ref gs);
                sv = proxyA.GetVertex(vertex->indexA);
                t.X = transformA.R.col1.X * sv.X + transformA.R.col2.X * sv.Y; t.Y = transformA.R.col1.Y * sv.X + transformA.R.col2.Y * sv.Y;
                vertex->wA.X = transformA.position.X + t.X;
                vertex->wA.Y = transformA.position.Y + t.Y;

                Vector2.Dot(ref d, ref transformB.R.col1, out gs.X);
                Vector2.Dot(ref d, ref transformB.R.col2, out gs.Y);
                vertex->indexB = proxyB.GetSupport(ref gs);
                sv = proxyB.GetVertex(vertex->indexB);
                t.X = transformB.R.col1.X * sv.X + transformB.R.col2.X * sv.Y; t.Y = transformB.R.col1.Y * sv.X + transformB.R.col2.Y * sv.Y;
                vertex->wB.X = transformB.position.X + t.X;
                vertex->wB.Y = transformB.position.Y + t.Y;
                vertex->w = vertex->wB - vertex->wA;
#endif

                // Iteration count is equated to the number of support point calls. 
                ++iter;
                ++gjkIters;

                // Check for duplicate support points. 
                bool duplicate = false;
                for (int i = 0; i < saveCount; ++i)
                {
                    if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
                    {
                        duplicate = true;
                        break;
                    }
                }

                // If we found a duplicate support point we must exit to avoid cycling. 
                if (duplicate)
                {
                    break;
                }

                // New vertex is ok and needed. 
                ++simplex.m_count;

                //simplex.m_v1 = vertices[0];
                //simplex.m_v2 = vertices[1];
                //simplex.m_v3 = vertices[2];
            }

            // in case of break -> save result
            //simplex.m_v1 = vertices[0];
            //simplex.m_v2 = vertices[1];
            //simplex.m_v3 = vertices[2];

            gjkMaxIters = System.Math.Max(gjkMaxIters, iter);

            // Prepare output. 
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance = Vector2.Distance(output.pointA, output.pointB);
            output.iterations = iter;

            simplex.WriteCache(ref cache);

            // Apply radii if requested. 
            if (input.useRadii)
            {
                float rA = proxyA.m_radius;
                float rB = proxyB.m_radius;

                if (output.distance > rA + rB && output.distance > PhysicSettings.FLT_EPSILON)
                {
                    // Shapes are still no overlapped. 
                    // Move the witness points to the outer surface. 
                    output.distance -= rA + rB;
                    Vector2 normal = output.pointB - output.pointA;
                    normal.Normalize();
                    output.pointA += rA * normal;
                    output.pointB -= rB * normal;
                }
                else
                {
                    // Shapes are overlapped when radii are considered. 
                    // Move the witness points to the middle. 
                    Vector2 p = 0.5f * (output.pointA + output.pointB);
                    output.pointA = p;
                    output.pointB = p;
                    output.distance = 0.0f;
                }
            }
        }
#else
        /// <summary> 
        /// Compute the closest points between two shapes. Supports any combination of: 
        /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output. 
        /// On the first call set SimplexCache.Count to zero. 
        /// </summary>           
        public static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input)
        {
            output = new DistanceOutput();

            ++gjkCalls;

            DistanceProxy proxyA = input.proxyA;
            DistanceProxy proxyB = input.proxyB;

            Transform transformA = input.transformA;
            Transform transformB = input.transformB;

            // Initialize the simplex. 
            Simplex simplex = new Simplex();
            simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);

            // Get simplex vertices as an array. 
            SimplexVertex[] vertices = { simplex.m_v1, simplex.m_v2, simplex.m_v3 };
            const int k_maxIters = 20;

            // These store the vertices of the last simplex so that we 
            // can check for duplicates and prevent cycling. 
            int[] saveA = new int[3], saveB = new int[3];
            int saveCount = 0;

            Vector2 closestPoint = simplex.GetClosestPoint();
            float distanceSqr1 = closestPoint.LengthSquared();
            float distanceSqr2 = distanceSqr1;

            // Main iteration loop.
            int iter = 0;
            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates. 
                saveCount = simplex.m_count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = vertices[i].indexA;
                    saveB[i] = vertices[i].indexB;
                }

                switch (simplex.m_count)
                {
                    case 1:
                        break;

                    case 2:
                        simplex.Solve2();
                        break;

                    case 3:
                        simplex.Solve3();
                        break;

                    default:
                        System.Diagnostics.Debug.Assert(false);
                        break;
                }

                // If we have 3 points, then the origin is in the corresponding triangle. 
                if (simplex.m_count == 3)
                {
                    break;
                }

                // Compute closest point. 
                Vector2 p = simplex.GetClosestPoint();
                distanceSqr2 = p.LengthSquared();

                // Ensure the search direction is numerically fit. 
                //if (distanceSqr2 >= distanceSqr1)
                //{
                    //break;
                //}
                distanceSqr1 = distanceSqr2;

                // Get search direction
                Vector2 d = simplex.GetSearchDirection();

                if (d.LengthSquared() < PhysicSettings.FLT_EPSILON_SQUARED)
                {
                    // The origin is probably contained by a line segment 
                    // or triangle. Thus the shapes are overlapped. 

                    // We can't return zero here even though there may be overlap. 
                    // In case the simplex is a point, segment, or triangle it is difficult 
                    // to determine if the origin is contained in the CSO or very close to it. 
                    break;
                }

                // Compute a tentative new simplex vertex using support points. 
                SimplexVertex vertex = vertices[simplex.m_count];
#if !OPTIMIZE
                vertex->indexA = proxyA.GetSupport(Math.MulT(transformA.R, -d));
                vertex->wA = Math.Mul(transformA, proxyA.GetVertex(vertex->indexA));
                //Vector2 wBLocal = new Vector2(); 
                vertex->indexB = proxyB.GetSupport(Math.MulT(transformB.R, d));
                vertex->wB = Math.Mul(transformB, proxyB.GetVertex(vertex->indexB));
                vertex->w = vertex->wB - vertex->wA;
                //vertices[simplex.m_count] = vertex;
#else
                Vector2 t, sv, gs = Vector2.Zero, nd;

                nd = -d;
                Vector2.Dot(ref nd, ref transformA.R.col1, out gs.X);
                Vector2.Dot(ref nd, ref transformA.R.col2, out gs.Y);
                vertex.indexA = proxyA.GetSupport(ref gs);
                sv = proxyA.GetVertex(vertex.indexA);
                t.X = transformA.R.col1.X * sv.X + transformA.R.col2.X * sv.Y; t.Y = transformA.R.col1.Y * sv.X + transformA.R.col2.Y * sv.Y;
                vertex.wA.X = transformA.position.X + t.X;
                vertex.wA.Y = transformA.position.Y + t.Y;

                Vector2.Dot(ref d, ref transformB.R.col1, out gs.X);
                Vector2.Dot(ref d, ref transformB.R.col2, out gs.Y);
                vertex.indexB = proxyB.GetSupport(ref gs);
                sv = proxyB.GetVertex(vertex.indexB);
                t.X = transformB.R.col1.X * sv.X + transformB.R.col2.X * sv.Y; t.Y = transformB.R.col1.Y * sv.X + transformB.R.col2.Y * sv.Y;
                vertex.wB.X = transformB.position.X + t.X;
                vertex.wB.Y = transformB.position.Y + t.Y;
                vertex.w = vertex.wB - vertex.wA;
#endif

                // Iteration count is equated to the number of support point calls. 
                ++iter;
                ++gjkIters;

                // Check for duplicate support points. 
                bool duplicate = false;
                for (int i = 0; i < saveCount; ++i)
                {
                    if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
                    {
                        duplicate = true;
                        break;
                    }
                }

                // If we found a duplicate support point we must exit to avoid cycling. 
                if (duplicate)
                {
                    break;
                }

                // New vertex is ok and needed. 
                ++simplex.m_count;

                simplex.m_v1 = vertices[0];
                simplex.m_v2 = vertices[1];
                simplex.m_v3 = vertices[2];
            }

            // in case of break -> save result
            //simplex.m_v1 = vertices[0];
            //simplex.m_v2 = vertices[1];
            //simplex.m_v3 = vertices[2];

            gjkMaxIters = System.Math.Max(gjkMaxIters, iter);

            // Prepare output. 
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance = Vector2.Distance(output.pointA, output.pointB);
            output.iterations = iter;

            simplex.WriteCache(ref cache);

            // Apply radii if requested. 
            if (input.useRadii)
            {
                float rA = proxyA.m_radius;
                float rB = proxyB.m_radius;

                if (output.distance > rA + rB && output.distance > PhysicSettings.FLT_EPSILON)
                {
                    // Shapes are still no overlapped. 
                    // Move the witness points to the outer surface. 
                    output.distance -= rA + rB;
                    Vector2 normal = output.pointB - output.pointA;
                    normal.Normalize();
                    output.pointA += rA * normal;
                    output.pointB -= rB * normal;
                }
                else
                {
                    // Shapes are overlapped when radii are considered. 
                    // Move the witness points to the middle. 
                    Vector2 p = 0.5f * (output.pointA + output.pointB);
                    output.pointA = p;
                    output.pointB = p;
                    output.distance = 0.0f;
                }
            }
        }

#endif
    }
}
